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AWSCBTT: A simple strategy for the attitude control and arm coordination of a maneuverable 
space robot with dual arms is proposed. The basic task for the robot consists of the 
placement of marked rigid solid objects with specified pairs of gripping points and a speci- 
fied direction of approach for gripping. The strategy consists of three phases each of 
which involves only elementary rotational and translational collision-free maneuvers of the 
robot body. Control laws for these elementary maneuvers are derived by using a body- 
referenced dynamic model of the dual-arm robot. 


1. INTRODUCTION 

I» th. design or ortit.l n.n.nverin, vehicle, (GHV) 

ststion assembly and maintenance, and ,f i*’)*,' co „J t adistinction with earth-based fixed robots, 

~ ««i«- •«. 

robot arm motions. / 

this paper. -« propose a simple strategy for attitude control andar. coord; '"“Xgln^ir.^scripUon 

task is presented. The paper concludes with a brief description of the work m progress. 

2. ROBOT AND TASK DESCRIPTIONS / 

Figure 1 shows the basic configuration of the CMV dual-arm robot under consideration For^im^ici^. the^ 

main frame of the CMV is represented by a maneuverable riqid body which provit es a 

assinn^ that°the"arms have only rotary joint, and .their motion, with respect to the base ate planar. 

The basic task tor the * robot consist, of placement of ~.r|»d rigid object, « . "marked J. 

«« r f rasir.iS' issr-ras f : - 

ST3 U £LS T -SS-S=^r 

the end-effector size and the mode of gripping. igure . f roach for both end-effectors is specified, 
planar dual-arm robot. In the first mode, a single direction of approach for both end effector P 

In the second mode, the end-effectors may approach the rod fro. oppo. “ rlutive o'nenta- 

- ^-n iB t, e i OSS 

the object). 

3. BASIC REQUIREMENT AND CONSTRAINTS 

Before discussing the problem of deriving suitable control strategies for the CMV robot to perform the basic 
task, we first consider the basic requirements and physical constraints associated -fid t e pro em- 
it) and r (t) denote respectively the compact connected spatial domains in the Euclidean 
‘ W “ Unk and the OMV base at time t. Their boundaries 


Let 7. (t) , Z . (t) , 


SPaC g tefbvihtr r.TrirB. WJ ^ , .iti.rd^ of theentire robot at time t is denoted by = 

U e ( r<3n< (t)U r Ctf )-. ' We assume that 7. (t) has a pair of specified gripping points ^(t) and ^ ( t ) on 

which'L time-invariant with respect to any fixed We ^ ^ t5 a 

co( !p* (t) ,£* (t) r) f the convex hull of the set >•!) lies in a pia • “ 0 ' -o 

specified direct ion of approach for gripping by the end-effectors of both robot arms. 


tho marked object, i-th joint, ]“ 


(t) 

3 

(t) = 


to be an autonomous or self-contained system, it is n-tural to introduce a coordinate 

es as the basic reference frame for the arm motions and the vision system. 

the arms. 

ay»l 257 


Since the CMV is 
system C which serves as 

the origin of C B is fixed at the root of one of the arms. 


Let ,e v (t) ,£^(0 ‘ denote the time-dependent 



basis 6 for C . We align e (t) with the axis of the first rotary joint of arm 1 (see Fig.l). 

t B _ 2 

Let p‘(t) and p 2 (t> denote the base points O and 0* of links 1 and 1* at time t respectively. The line seg- 
ment info « co(?S'(t> V(t>}> and normal r» <t> - e (t) define a plane n (t) , where ru<t) specifies the heading 
of the R 0MV robot aP time t. The position R of the-' end-effector of arm i (may be taken as the tip of the second 
link of arm i) is denoted by £^(t), and the deviations £^(t) - gMt) and r^(t) - r^U) by A£ A (t) and A£(t) re- 
spectively. 

Now, the basic requirements and constraints associated with the control of OMV robot can be stated as follows: 

(a) Before the end-effectors are in contact with the specified gripping points of the object, any OMV maneuvers 
and arm movements must be collision-free. This implies that 

(i) }} (t)0 3. v (t) => 0 (no robot-object collisions); 

R 'o 

(ii) r II OE.. (t)u 3E.. <t))ini U 3E T - (t) ) ! - 0 (no arm-arm collisions); 


(ii)[ u 2 (3j: Ji (t) U DJ: Li <t))in( 


(iii) 3I ; (t)D 3Z B (t) = 0 and JE^U) D 3Zg«t) = 0, j=2,2* (no second link (joint ) -base collisions) 

at any time tElO^l, where ^ corresponds to the first time when A£ X (t^ = 0, i-1,2 and AnU^ = 2. (or H A H {t i } II 

* E.SlApNt )llc 3 given positive number). The foregoing conditions may be relaxed by allowing point contacts 
with^zero velocity. In the case where a specified clearance between any two components of the robot must be 
maintained, we may enclose each member by a boundary layer with prescribed thickness, and impose conditions (i)- 
(iii) to the outside boundary of the layers. 

(b) Each end-effector should tend to its designated gripping point in a smooth non-oscillatory manner during 
its final approach. This can be fulfilled by requiring 1 1 A£ A ( - ) j | and l]Ar l (-)|| to be smooth strictly monotone de- 
creasing functions of t over some • -hinterval [t* ,t ) C [0,t ] , and || AjaMt ) ||< e . , i-l» 2 and || Antt^H^ £ , w ere C Q , 
f- and f- 2 are specified nonnegative nu-.bers. To ensure acceptable relative velocities between the end-effectors 

and the gripping points of the object when they are within the gripping range, additional velocity constraints: 

| [ (*.£*■ ( t ) | j 4 ~ ,i«l,2 and llAntt^jjs may be introduced. 

(c) To achieve complete autonomy of the OMV robot, the control strategies or control laws should depend only on 
on-board sensor data. Moreover, they should be sufficiently simple so as to permit on-board real-time implemen- 


Evident ly , the incorporation of the foregoing requirements and constraints into the formulation of any control 
problem leads to formidable difficulties. A basic difficulty is that the characterization of the class of con- 
trols which generate the co 1 lis ion-f ree maneuvers and monotone approach is not readily obtainable. In what 
follows, w n propose a simple approach which bypasses the abovementioned difficulty. 

4. PROPOSED STRATEGY 

The basic idea ii to decomoose the robot control problem into three phases: 

(PI) Alignment Phase: The objective is to maneuver the CMV so that its heading h R is aligned with the object's 
gripping direct!, on n". Moreover, the arms are prepositioned to achieve the required end-effector orientation 
and position so that°the object can be grasped by a subsequent straight-line translational motion of the OMV. 

Since it is difficult to achieve collision-free OMV maneuvers and arm movements when the object is close to the 
OMV robot, we propose to move the OMV sufficiently far away from the object before initiating any alignment maneu- 
ver and arm prepositioning. 

( p2 ) Acquisition Phase-. With the arms' joint angles locked in the preset values, the CMV moves along a ^ olli " 
sion-f ree straight-line path to rendez-vous with the object in a monotone manner. ( The attitude ,.r d translation- 
al motion control systems at the OMV base maintain the deviations ,i An (t )|| , j| An ( t ) jj ,|j c-£ L (t )|| ,j| A£ (t)!| , i = l,- within 
the acceptable values at all times during the rendez-vous* 

(PJ) Task Phase: -After graspping the object by means of the end-effectors, the joint angles are again Locked, 
-hen the OMV moves to the required destination and places the object there with the specified orientation. 

Finally, the UMV backs away from the object along a straight-line path. 

The choice of .= t raicht -I ine paths for translational motions is motivated from the fact that complex maneuvers 
of the CMV in space should be avoided , since such maneuvers could be catastrophic in case of control system failure. 
The proposed locking of all joints during any base attitude alignment and translational maneuvers a'- -- - the possi- 
bility of undesirable arm motions induced by the inertial forces and moments. 

In what follows, we shall present a control strategy for each phase. For simplicity, only the case with 
planar motion will be considered here. 

4.1 Maneuvering Strategy for Alignment and Acquisition Phases ■ • 

r t n _ /a a -j a a ) an( j " (O r )C1R : denote the spatial domain of tho OMV robot corresponding to a 

- L*2 r l*' 2 #/ o R CO 

given set of angles ” and a specified r (position of the centroid of the CMV base relative to the inertial 
frame). Let T(r ) — =1 | " (0,r ~ C< f which corresponds to the set of all points in R swept out by the 
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robot body as 0 varies through its admissible values specified by the set : ad while keeping r^ o stationary. 

To simplify the ensuing development, we assume that the object E^ is stationary with respect to the inertial 
frame. If the GMV robot and the object are sufficiently close to each other initially such that Hr^) 0 ^ o ^0» 
then the objective is to move the robot to a new position specified by r* at which I (r^ Q ) H E q * 0 and 
inf {|jx - x'i| j x € Hr ) , x' • € £ } > e > 0, where e is a given nonnegative number. To avoid a complex col- 

lision-free maneuvering problem*^ we restrict the maneuvers to straight-line translations without robot -body rota- 
tions and with locked joint anqles. Now, we give a simple collision-free maneuvering strategy by representing 
the robot arms 7 . and E . by line segments and the base T. by a closed rectangle as shown in Fig. 3. Moreover, 
the object E Q iaassu»ed A £o be a compact convex set with interior points (If 7 ° is not convex, we consider coO.^) 

instead of I ) . We also impose the following geometric constraints i 
o 

(Cl) Link- length Constraints i £>«.,, and *■, * *■< where w is the distance between the rotation axes 

of joints 1 and 1*. The latter condition implies thata collision between links 1 and 1* is impossible. 

(C2) Joint-angle Constraints; Let € be a specified small positive angle and 


\ - cos" 1 tys^), e 1# - cos \ - 2tan ‘ , (i-i L ^) * ®i . 

where i_ 

s . (J, i+ l 2 + h)/2, s’ - U lt ♦t 2 . + h>/2, y •[(s-^Ma-ljKs-hl/sl 3 , 

We require: 

0 - ef $ 01 < TT + 0 X + e, -0 1 , + c < 9 1( < " - 0 1 , - e, 


- 2tan "‘(i^7r)* (1) 

l 

r - [ ( 3 * — l ,) (s' - l 2 ,) (s' - h) /s' I*. 

( 2 ) 

(3) 


and 

|0. | <ir - F, i . 2,2*. <4) 

Condition (3) along with constraint (Cl) imply that a collision between arm 1 or arm 2 with the base is impossi- 
ble. Condition (4) avoids the possibility of link 2 (link 2*) folding back onto link 1 (link 1*). 

Let 0° and r° denote the initial set of angles P° 3 *- t * ,on t * le ^ ase “ centro ^ re_ 

sp€ctiv©ly* Evidently, cod (Q° # r° )) is a closed convex polygon# Q The straight-line collision-free maneuver- 
ing problem can be stated as ^ollow§? Given Z (0 # r^ o ) such that *£^ 0 ) ^ find a direction vector n, 

such that Z (0°,r° + an) PI E - 0 for all real numbers a>0. 

R — —co — o 

A solution to this problem is given by the following maneuvering strategy: 

Case 1 : If E and co(E (Q 0 ,r 0 )) have no common interior points, then we move the robot along a straight-line 
path in the direction ir^ until 

inf { ] | x - x* || : x € T (r * ), x*6^ )• e , (5) 

where n is the normal (directed toward co(E f 0°,r° )) of any line separating the convex sets E q and co(E R (0 ,£^ Q )), 

and r* is the new position of the base-centroid” '(see Fig. 4a). 

CO o o 

Case 2: If E and co(E O ,r° )) have common interior points, but E <£co(I (0 ,r ) ) , then there exists an 

O R — —CO ° * —CO 

"exit edge" E of the polygon co(E (0°,r° )) (i.e E is an edge which does not correspond to any link or edge of 
the base) such that E Pt E f 0. R ” CO Let L(E) denote the line containing E, and iPy the projection operator 
from IR 2 onto L(E) in the°direction v. The maneuvering strategy for this case is to move the robot along a 
straight-line path in the direction -y until condition (5) is satisfied, where y is any direction such that 

& ~(E n co (E (0°,r° ) ) C int ( E ) , (6) 

vo R — co 


where int (E) denotes the interior of E(see Fig. 4b). In general, there may exist a cone of directions y which 
satisfy (6) . 

Case 3: Suppose that E Cco(Z ( )°,r° )). Let E be the exit edge of co(Z (0°,£° •) ) associated with i. (i.e. 

c ‘ o R — — co K — u 

the exit edge associated with the closed domain DCco(Z (0^ ,r^^) ) containing whose boundary 3D is composed of 

E and one or more links and base-edges) (see Fig. 4c). R Suppose there exists a°direction y such that 

rp~c: ; C int (£) ( 7) 

y o 

arid 


co ( E U^-CE ))fl E (0°,r° ) = 0 (8) 

o y o R — —co 

are satisfied. Then we move the robot along a straight-line path in the direction -y until condition (S) is 

satisfied. Here, condition (8) implies that E can be projected into L(E) in the direction y without any oh- 

o 

structions from any part of the robot. 

Case 4: Suppose that E C co ( E (Q°,r° )) and there does not exist a direction y such that (7) and (8) are satis- 
fied simultaneously (see'Vig. 4d$\ _C ° Then it is impossible to achieve a collision-free straight-line maneu- 

ver for the given set of angles 0° = (0^ ,0°, 0^, »0 O , , 9 ^) . In this case, it is necessary to change 13 until both 
conditions (7) and (8) are satisfied for some direction y. We propose to accomplish this by altering the joint 
anales only without introducing a base rotation. The joint angles should be adjusted such that the sequence of 
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domains {D ,',j-l,2,... } (defined in Case 3) generated by a sequence of joint-angle settings satisfy DjCD.^, 

i-1,2 1 , 

u < ' ►K. rnhnf sufficiently far away from the object, the next step is to perform a rotational maneuver 

Haying m ith the object’s gripping direction n while holding all the joint angles at their 

to a ign the WV heading n J^ed by a tran 3 lationaI°maneuver along a line parallel to the line ccn- 

initial values (see Fig.SaK Thxs is^ollow Qn the robot bage is ali gned with a corresponding 

taining the gripping poin Eg -Ji^Sc) Finally the joint angles are adjusted so that the object can be 
TAZ.T* stUht-Un.'U.n.l.tionel cl the * -UU holdin, the «ioW. 

tn. CUV bee. stationary (see Hg.5d). This completes eh. eltqnment phass. 

in th. acquisition pha... th. control .».«» quids, th. c«v .long a should^ 

th. object while Iceepijg .11 the Joint '“/"notomcali; with t as ... . Sine. th. .l.n.n.ary 

Z'ZTrl'Z SSS t'i^aSLit ^d .:S!.i.ion phases require controllin, at ... tin variable, at a ties, the 
control problem is greatly simplified. This aspect will be discussed m the following section. 

4.2 Control Laws for Elementary Maneuvers 

T ° r v ; r^r^i .r j°; .: h : .irsrzLJi-^.s 
Tr 0 -Mr-r- ----- “* 

given by 


- il,2 ' m i~cl * — * 

" *1,2 * -l, cl* — 1 ,2 


" ^O.cl* -0,1 


^ ^ * (I l-l' = - 5 



f 

-1,2 

‘ m 2^:2 * 



—1,2 ■ 

‘ — 1 ,c2 " -1,2 

x 2 

Link 1* 

f 

^ ,1* 

- — 1 * ,2 * ‘ "X* 

^cl’ 


i).L’ 

- — 1 * ,2’ V ^-1* 

,c 1 * 

Link 2* 

_ * f 

-1* ,2 

• ” m 2 ’ ^-c2 * 2.* 



n ,. (2 

• ” — 1 * , C 2 ’ ^ lx 

• 

L.i n k 0 

(QMV Base) 





- f - m v 

* f 


-1.0 

-0,1 o -CO 

“ 


*1,0 

- M , , + r , 
-0,1' —o' ,co 

X f 
^-0 


- l 2 i 2 -« 2 * ( 1 2 ) “ 9J 

v * 0 , 


X f - I . . <L. , - 'iJ, , ‘ ( 1 , , 'D, , ) = 2. ; 

-0*,ci’ ^0,1' 1* -i' -1 1' -1’ 


* (1 ,. <£;>• ] 3 2.* 


, .. < r -r v E -I .j - IK X (I d ) t N = 0 , -1*1 

!l Ef 0 " *o,l' -<j' ,co — 0 , 1 1 ^o,co -1,0 o-o-o o-o 

where ^ is th. velocity „ the^.ntro.d .Ml* 

is the mass ot link i, - L , a -i * i+ l ve iGc itv and centroidal inertia tensor of link i respectively; de- 

N .is the coupling moment applied **••*. L V Y . — c 

a l cort£rol force actinq at the centroid of link 0. 

For the case of a planar CHV robot, all the joint axes are along the z or * axis Let T. and J denote^ne 

i:;?L.'sr iz.*r ‘-m i ( r«!r M , ^, , -'sn 


' L — o,c 1 


-' m 2 l £l fC l ' ^>,cl “ -l,c2 J 


2 -2 “ n 2-l,c2 — c2 ~ 2 -z ' 

ij. * V lo> ,cl’ ' itl’ ■ -I’.Ol ~ Vyd’ ' -X , .C2’*" < ^1* " l l’ 

I,. k 2 . - VH’,c2’ ' ic2* = W 


O “O *1-1 *2 

- , ( r , 


* I 2 .i 2 . - V^, = o - Io,cl )X Kl “ m 2 —1 , cl - ^,cl ' — 1 , c2 * ^,co )X t 2 


( ^.,co ' — o 1 , c 1 ' ^ il' ‘ a :’ -l'.cl ■ V.cl* ‘ “X ' rC2 ' ’ V,:o H ^ 
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<19) 


Vcl 


♦ W_ V 


2 c2 1 


where uh « o» e . Equations (14)-(19) constitute a complete description of the motion of the planar GMV dual— airm 
robot. 


To obtain explicit forms for (14)-(19), we choose the body coordinate system with basis 0^ ■ {e^(t),e (t) , 
e (t) } and with the origin O on the axis of rotary Joint 1. Moreover, e f t } is directed from point 0 to~^o*. 
Tnia choice of origin 0 is preferred over that at the base-centroid, since the latter is usally not precisely 
Known. 'Now, we introduce the joint angles 0 , l* 1 ,2 , 1* ,2' , and the OMV base attitude angle 0 as shown in Fig* 

3. Thus, the angular velocities (u. are related to the 6'a by ° 

i 3 


- $ A 


1' 


W 0 2' 


0*0. 


♦ 9. 


< 20 ) 


Note that, all the angles 0^, with the exception of 0 , are body referenced. Thus, they can be measured by nwemns 
of body-reference sensors. The base attitude angle 9 must lie measured by means of an inert ial-roference sensor. 

Since the CHV robot is autonomous , it is natural to use the hody coordinate system as the basic reference 
frame for the arm motions and for observations of the environment from the (XV robot. Expressing the position 

vectors r , with respect to the baais B , wo have 
t ,cj t 


r « l ,{(csl)e (t) + (snl)e (t)l, 
~o,c 1 cl -x — y 


r - x e (t) y e 
-o,co o,co~x 'o,co-y 


-y 

<t) , 


«• ,cl' 


f. ,,l|csl')e (t) t <snl*)e (t) l ( 
cl* “X — y 


- (x - w)e (t) + y e (t) , 
,co o,co —x 'o,co— y ’ 


r. ’ f ((<:sl2)e (t) t (snl2)e (t)l, 

— L ,c2 o2 —x — y 

r., ,, - < ( (csl'2*)e (t) ♦ (an 1' 2 Me (t) I, r, , - ( l (csl)e <t ) + (snl)e (t> ) , 

— I',c2* c2 ' — x -y — l, <;1 cl l -x -y 

r , ■ (< ,, - <, , ) { (csl* )e (t) +• (snl')e (t)}, 

— 1 ' , c 1 cl' 1 ' — x -y 


121 ) 


where sni sin 9^ , cai ■ cos 0 . , an i j = sin((l, > *1 . ) and cs i j =* cos (9 , t 9 . ) . In wr i t ing down (21), we have atia-xsr 

ed that the centroid (marked "®" in Fig.l) l of Any link t of arm t or^2 is located inside link i along the line 


nij » sin(9, *9.) anil c:i i ) =* cos (9 . ♦ 9 . ) . In writing down (21), we have atii-xir 


segment connecting the joints i and i+1. 


The centroid of link 0 (OMV base) specified by r has the time— 

-o,co 


o,co o,co 

Let r (t) » x (t)e (t) + y (t)e (t) denote the vector directed from the origin 0° of the inertial frame to thu 
origin 0 of the Body coordinate system C . Thus, 


(x -y 0 - 2& y -x 9*)e (t) ^ (y ► x 9 * 2 x 9 - y 0*)e (t). 

" 00 00 00 —x o 00 00 o o -y 


f22) 


The accelerations of the link-centroidn v , i=>o , 1 , 2 , l ’ ,2 ' can he obtained by differentiating (21) (see Appendix 
for their explicit expressions). Substituting the explicit expressions for v and V leads to the following 
equations of motion for the (XV robot: 


H(c^) V (cjyjj = u, 2 0 


where * 

<V 9 2 

! ,9 1* ' 

e,,,0 , 

2* 0 

x »Y 

0 0 

) T , u * 

(l Y 

V 

T A T 

1 * <T,,r,,r,,,i ,,r ,f ,f , 

1 2 l' 2* c cx cy 

r '» f e (t ) t f e (t) . 
— c cx— x cy— { 

V » (v ,.. 
ments, and 

..V J 1 

is a 

vector 

-valued function 

of g and g representing the centrifugal 

and Coriolis forces and rmo— 

«<a> 

is a 

7 “7 matrix 

of the 

form: 







' h ll 

h 12 

0 

0 

h l5 

h lh 

h 17 1 





h 12 

h 22 

0 

0 

tl . 

J 3 

h 

20 

h 

.! 7 





0 

0 

h n 

h (4 

h j> 

h . 
(*) 

h 37 




H ’ 

0 

0 

h 34 

h 44 

h 45 

h 

4t. 

h 47 


(24) 



h hl 

h r ., 

h, 
r > ! 

h S4 

h, r 

h. 

h r 





h 16 

h 29 

h |f, 

h 49 

V, 

h 

nh 

n 





. h 17 

‘ h 27 

h 37 

h 47 

h 7 1 

!) 

h 77 . 




The explicit expressions for the elements of H ami the components of V are given in the Appendix. 

Now, we consider the problem of deriving sut table control laws for the elementary maneuvers using *he mathe- 
matical model described by (22). Since these problems tor all the elementary maneuvers are intrinsically iden- 
tical, we shall discuss only a specific case to illustrate the basic ideas. 

Consider the problem of deriving a control law for aligning the uMV beading n with a specified grijfing di- 
rection H while keeping all the joint angles and hase-cent rruil position st.it ionary. In this case, we regtii re- 
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( 25 ) 


«[(t) « < 0 ,o,o,o,(Mt) ,o,o) T » y.(t) , 

whore 7 w ,d , i«l,2,l\2* arc specified constants. Thus, equation (23) reduces to 

° ° 1 .. . ( 25 ) 

h 55<a> u 0 “ “V^ * l c ' 

( 27 ) 

h i5 ta)° 0 - + V 1 i rj * 

ut 0 d denote the base anqle such that ^ * H R , and A0 q * 0 d - 9 q . Equation (26) can be rewritten as: 

° .. . ( 28 ) 

h 55 (a)/,o o =• v 5 (jr.2> - t c . 

A simple control law for (28) is qiveri by 

( 23 ) 

t c - v rj <o:, 2 > ♦ hr j5 ( a><V°o f K r" V' 

( 30 ) 

u t * v.(.2,a) * h. 5 <{p(K p :.0 o t W 1/,. 

The fecfoin, control Uwa <„. and <». *P«- « f * “ 
shown that by choosing K and K properly, -uch con ro leg and the base-centroid position remain 

perturbations (see [)!, Chapter r 6). To ensure that all he )Oint control law Spending on the 

at their specified values during the heading aiignmcn ma f their specified values may be used, 

instantaneous deviations of the joint angles and base-centroid position from their P 

5. CONCLUDING REMARKS 

The Key idea in the proposed control strategy ^ 15 VsTZlZltTr, 

maneuvers into a sequence of elementary maneuvers invol g ooerations in a space environment in which 

maneuvers are simple to performed they are particularly J ions , they are being 

safety is a major factor. Although the results pres experiment involving a planar dual-arm maneuverable 

extended presently to the general three-dimensional case An ^ time, 

robot which is levitated above ground by an air bearing is in the planning stag 
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APPENDIX . . , c 

, c , _ . t (*■ i - a „ (t) e (t) = -9 e it), the accelerations of -~e 

By direct computation using (21) and the relations: - - Q _ y < _y o-x 

link-centroids are given by 

- •• _u _ ... •: . v ti - i X :: -•/. __*•;) £<*). 


V 5 

-CO 

r r 

— o 

+ r 
— o , 

CO 

= r 
—a 

- (y 

) o ,co 

: «* ;< 
o o. 

CO o' 

•• t) 
— X 

* 1 x 

o , 

CO ( 

D * <■ 

J,CO 



.. 


.. 

- • f M 

+. 

csl t 

(M + 

r ) , ) snl 

:e 

(t) - 


V , 3 
—cl 

1 r 
— o 

> r 
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v . , * r + we (t) + r , , , + r , ,, » r - <w6 2 + t , (0 ♦ 0 , f csl * 4 ?. ( 0 4 0 )snl ' + l , <0 4 0, , ♦ 0 , )snl*2* 

-c2* -o —x — 0 , 1 ' ~l',c2* -o o 1' o 1* 1’ o 1 c2* o 1' 2* 

4* +6,, +6,,) 2 csl»2')e (t) 4 {w0 -*,,(6 + 9,,)snl* - l (0 ♦ 6. , + 6,, ) 2 snl*2* ♦ , (0 ♦0.,)csl* 

c2' o 1* 2' -x o 1' o 1* c2 o 1 2' 1 o 1 


+ *c2* ( W ^ ,1 ’2 , >V t) ' 

where r (t) is given by <22). 

“O 

The elements h^ (cjJ of H (<j) are given by 

h ll " X 1 + X 2 + m l*cL + m 2 (? 'l + 29 l l c2 C32 + ? c2 ) ' 

h 12 “ h 21 " X 2 + m 2^c2 a c2 + V a2> ' 


h » I 4 I 4 n { l* 4 4 (y snl + x csl) (4 m { ■’ 2 4- 2 l. I _cs2 * i- x (4, csl * 4 cs!2) + y (4, snl + > ,sr»12) 1, 

15 1 2 1 cl cl o o 2 1 1 c2 c2 o 1 c2 o 1 c2 


h,, ** h., ® -m,4 ,snl - m„(4,snl * 4 _snl2) , 
16 bl 1 cl 2 1 c2 


h 17 * h 71 * ro l'cl CSl + 1B 2 <? 'l C31 + J c2 csl2) * 


h 22 “ X 2 * m 2*c2* h 25* X 2 +m 2 l c2 a c2 * V 32 +X o Csl2 +y o SnU) ' *26 ' ‘ m 2 'c2 3n12 ” *62 ' *27 ”V c2 Csl2 ‘ *72 ' 

(A- 10) 

h 33 - x l,>X 2 , ♦ « v l* eL , + » 2 ,<lJ. ♦2* 1 . 4 c2 * Cs2 ’ *^ 2 ,), h 34 -h 43 - I 2 . ♦■ 2 .»- c2 .<»- c2 . (A-ll) 

h 35 ” X l* * X 2* + ? C 1' ( y o f:n1 ' * <x o + w)csl') )+ > 2V. 1 ,y. c2 ,cs2’ 4 4 (x q 4- w) ( , csl* 4-J.^.csl ’ 2« ) 

4- y U, .snl* 4- 4 , snl' 2 * ) } , (A-12) 

o 1* c2* 

h 36 * h 63 * " m l* l G V mV * m 2* ^'V Snl * * ^ C 2 » sn1 ' 2 ' * ' h 37 ’ h 73 “ m l 1 ? ‘cl* CS 1 * f m 2 ■ * * 1 « csl # * * c 2 • csl * 2 * J • * A ~ 13 * 

h 44“ X 2* +m 2* i c2*' h 45” X 2* +n, 2’ t c2* U c2* "■ V 92 ’ Mx o + w)c31 ' 2 ' +y o Snl ‘ 2,} ' (A “ 14) 


1« J - ,l 3n1 ' 2 '/ 

46 64 2* c2' 


h 47 “ h 74 “ m 2 * ^c2 * cs2 -’ 2 ' * 


h = I 4 I 4- m. 4 , ( 4 , - x csl - y snl) 4 m, { 4 2 4 2 4 4 cs2 4 i 2 - x U.csl 4 ?. csl2) 

51 1 2 1 cl cl o,co J o,co 2 1 1 c2 c 2 o,co 1 c2 

- y (l,sni > l snl2)}, (A-16) 

o,co 1 c 2 

h 52 * X 2 + Vc 2 (f c 2 + J ' 1 CS2 ' X o,co CSl2 ' y o f co ynl2) * 

h 53 = X l* + r 2* +m l , 9 'cl‘ {i cl* * (w ’ X o,co )csl ' ■ y o,co Sn1 ' ! " m 2 * f 4 l* f 2! ’V l c2' Cs2 ' “ ? c2 * " (W “ X o,co ) ( S* C51 ' 

4 2 csl'2* ) - y (l. .snl'. 4 1 - , sn l * 2 ' ) f , iA-17) 

c2* o,co 1 ' c 2 ' 

h =l 4m_,J._,{?._ l + il, 1 cs2 , 4(w-x ) csl* 2 ' - y snl' 2'}, (A- 18) 

54 2* 2* c2 c2 1* o,co 'o,co 

h,. * I 4» (1‘ 4 (x *x ) i ,csl 4 (y -y ) l snl - x x -y y }♦ m U 2 ♦ 2! 1 cs2 4 t 2 

55 T 1 cl o o,co cl o o,co cl o o,co o o,cc' 2 1 1 c2 c2 

+ (x -x ) (i.csl 4 l ,csl2) ♦ (y - y ) U.snl 4 l snl2) *x x - y y ) 
o o,co 1 c 2 o o,co 1 c 2 o o,co o o,co 

4m,,(!. 2 „4(x 42w-x )? , , cs 1* 4 (v -y ) 1. snl 1 + (x 4w)(w-x ) *y y ) 

1 ' cl* o o,co cl* o o,co cl o o,co o o,co 

+ m^,[i 2 t 4 2 4,, l ,,cs2* 4 4 (x t 2w - x )( « cs 1 * 4 i cs 1 ' 2 * ) 4 (y - y )('! .snl 1 4 ?. , snl'2 ' ) 

2 ’ 1 * 1 * c 2 * c 2 ' o o,co 1 * c 2 * o o,co 1 * cx 

+ (x 4 w) (w - x ) - y y ) , 1 A-l'J) 

o o,co o o,co 

h = m, (y - 4 , snl) 4 m (y - sn 1 - snl2 ) ♦ m, , (y • . ,snl*} +in., (y -2,snl'-?. ,snl*2*), 

56 1 o,co cl 2 o,co 1 c2 1* o,co cl 2 o,co 1 ' c 2 

• A -20 ) 

h^_ = n, (J ,csl-x ) 4 m, [i.csl 4 cs!2-x ) 4 m, , (w 4 , • csl ' - x n f . Q ) + m 2 i (w 4 J.« icsl * 4 4 _ 2 • csl ' 2’ 

57 1 cl o,co 2 1 c2 o,co 1 C1 o,co ^ 1 

-x nrn> , fA-21) 

O f c o 

hrc = >. .snl - in < i snl ♦ l _snl2) -m, , - , .snl* - m.. (^..snl* + l jSnl^* ) - m y - mv, (A-22) 

6 b . 1 cl 2 1 c2 1* cl 1 2 f 1' c2' o o,co T,o 


h 66 = h 77 = m T* 


h^ c = m.l ,csl + m_ ( l. csl + l cs!2) 
75 1 cl 2 1 c2 


♦ m, # (w ♦ i ,,csl*) fn, t (w^ lf csr + l ^.csi*2* ) -f m x + m x , (A-24) 

1* cl* 2* 1* c2 o o,co T o 


I - I +■!_ +1^ +• I, . + I.., in,. = m ^ m, + and all the remaining h . . *s are zero. 

T o!21 # 2 , Tol21 , 2 
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The components of ViJ.g} «r« given explicitly byi 

V 1 - ■ 1 ‘AV«o S o linlt ^oVv o 5jc.l) ♦ » 2 ^ 2 V 0 ®o + X o § o ,a i SnW A c2 8nl2) + (2 *oV y o®o )(t l C91 

+ t c2 csl2) - (2(§ o + 0 1 )9 2 + 02)t i a <;2 fln2}, (;v ■ 25, 

V 2 " B 2 i c2 U l (5 o +S l )ifln2 + (2 y o § o + X o § o ,snl2+ (2i o®o _y o 5 i >CSl2} ' <A " 26> 

V 3 - m 1 ,t cl ,{( 2 y o 0 o + <x o + w)8^)snl' ♦ (2iJ Q - y o §*>cal‘ }♦ ® 2 , {(2y oV (x q + w) (l^snl* ♦ £ c2 ,snl*2* ) 

♦ < 2 x o 5 o -y o §J)<£ ll csl' + a c 2 ,csl‘2')-(e* l *2<e o f5 l ,)5 2 ,)sn2M, ^ 27 ’ 

V 4 " “ 2 » 1 c2* U 1'^o + § 1' )2 “ n2 ‘ + (2y o®o + < x 0 > w) 8o ,snl *2’ + (2 *o®o ‘ y o & o >C81 * 2 ' } ' <A " 28 ’ 

V 5 - ■ n i ,,X o ( co- t cl CSl)Ui oV^ - t cl < ®o* 6 l )lanl) * (y o,co-y” l><2 ^o + X o 5 o +l cl ( V«l ,2c * 1)} 

’ B 2 { (X o,co ‘ V 91 " 1 c 2 CSl2) , 2 i o®o : y o®o “ l l {h o + V 2 “ nl ' *c 2 ( ®o * *1 * V 9nl2) + ^o.co' V° l ” ^c 2 9nl2) * 
( 2 y 9 + x 9 s + 1,(0 >9 ) 2 cb 1 ♦ l .<& ♦ K + §,)’c« 12 >} - V ( <x 0 , co “ w ’* c l' cs1 ' >(2x oM “ y o 0 o 

4 o O OO lO 1 C* 0 4* ’ 

- l cl ,(0 o *© 1 . > 2 •nl*) ♦ <y o ,co’ l cl* snl,)(2y o®o * X o 5 i * *cl’ ( W 2c91 * +W0 o )} 

- b 2. { (x o,co- W - l V CaV “ 1 c 2 ' C9l ’ 2,) (2x O 0 O " y o 0 o ‘ l l*^o*®l' ,,sn1 ' “ 4 c2- <0 o + 0 l* +0 2- ,!8nl,2,) 

+ <y o Co’ 1 !'™ 1 ' • l c2* 3nl ' 2,)(2y o 0 o + X o § o + *l’ ( VV 2cSl ’ * ^’‘V^^V 2091 ’ 2 ' 

o,co a '•* (A-29) 

V 6 ■ -M T <29 o y o ♦ x o 0^) - Vd * n 2 V ( ®o + V C#l ' * 2 WVVV 2c912 “ ( V*el' + B 2' l l' ,( W ,t>r 

* m 2' i c2' (4 o + - (m^ ♦ « 2 .)-Sj - Vo.co ^ (A ' 30) 

V 7 “ M T !20 o X o “ y o 0 o ) “ Vcl + l »2 t l )(0 o + 0 l )29nl " m 2 ll c2 t0 o +0 l 4 '^2 )23n12 " {ra l • * C 1 * + m 2 • £ 1* 1 ( ®o * § 1 • Y 8nl ' 

m 2 * ^c2 ' ^ 0 O + ^1 ' + 0 2‘ ^ snl ' ^ ' “ m o y 0f co 0 o* (A ‘ 31) 

Note that H (q) is not the manipulator mass-inertia matrix. Its nonsymmetry ( i.e. h^i*) - h^ (g^) 5 , i-1. . ... 

7) is partially due tTThe fact that the control force f and the acceleration of the base-centroid are ex- 
pressed in terms of the body coordinate system C with basis 8 The moment r x £ c ?«, 5"q, 

nonsymmetry of H(g). It can be shown that the Manipulator mSss-inertia mat 7ii « «a> 13 related to H( ^ 

by 

\ l A °1 , J r 4 °j ( A- 32 j 

K ( a ) = H <0.) » 

0 <2 j L 0 P . 

where I is the 4*4 identity matrix, and 
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. 0 0 
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1 0 


0 0 
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(b) 


Fig. 2 Moles for gripping a rod by a planar 
dual-arm robot. ta) qrippinq with a 
single direction of approach for both 
end-effectors, (b) grippinq with dif- 
ferent directions of approach for the 
end -ef fee tors . 


Fiq.3 Simplified planar maneuverable dual- 
arm robot. 
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